#include "motorcontrol.h"
#include <stdio.h>
#include <gpio.h>

#include "timemanage.h"
#include "taskMotor.h"
#include "tim.h"
#include "com.h"
#include "tasksendudp.h"
#include "taskICU.h"

uint8_t motorControl1Action = 0;
uint8_t motorControl2Action = 0;
uint8_t motorControl3Action = 0;
uint8_t motorDirectionControl1Action = 0;
uint8_t motorDirectionControl2Action = 0;
uint8_t motorDirectionControl3Action = 0;
uint8_t motorSpeedControl1Action = 100;
uint8_t motorSpeedControl2Action = 100;
uint8_t motorSpeedControl3Action = 100;
uint8_t flagMotorControl1Update = 0;
uint8_t flagMotorControl2Update = 0;
uint8_t flagMotorControl3Update = 0;
uint8_t flagTimeListUpdate = 0;
extern osThreadId timerMotorControlHandle;
/******************************************************************************
 * FunctionName : taskMotorControlInit
 * Description  : 电机控制任务初始化
 * Parameters   : void
 * Returns      : void
*******************************************************************************/
void taskMotorControlInit(void)
{
   TIM1_PWM_SetFrequencyAndDutyCycle(16000,motorSpeedControl1Action);
   TIM2_PWM_SetFrequencyAndDutyCycle(16000,motorSpeedControl2Action);
   TIM4_PWM_SetFrequencyAndDutyCycle(16000,motorSpeedControl3Action);
}

/******************************************************************************
 * FunctionName : taskMotorControl
 * Description  : 电机控制主任务
 * Parameters   : void
 * Returns      : void
*******************************************************************************/
void taskMotorControl(void const * argument)
{
  /* USER CODE BEGIN taskMotorControl */
  /* Infinite loop */

  while (1)
  {
    setAction();
    timeControl();
	sendudp();
    osDelay(200);
  }
  /* USER CODE END taskMotorControl */
}


/******************************************************************************
 * FunctionName : setGPIO
* Description  : com接收设置指令
 * Parameters   : void
 * Returns      : void
*******************************************************************************/
void setAction(void) 
{
    if(flagMotorControl1Update == 1)
    {
		  taskMotorActionControl(1, (MotorAction)motorControl1Action);
		  taskMotorDirectionControl(1, (MotorDirection)motorDirectionControl1Action);
      flagMotorControl1Update = 0;
    }

	if(flagMotorControl2Update == 1)
    {
		  taskMotorActionControl(2, (MotorAction)motorControl2Action);
		  taskMotorDirectionControl(2, (MotorDirection)motorDirectionControl2Action);
      flagMotorControl2Update = 0;
    }

	if(flagMotorControl3Update == 1)
    {
		  taskMotorActionControl(3, (MotorAction)motorControl3Action);
		  taskMotorDirectionControl(3, (MotorDirection)motorDirectionControl3Action);
      flagMotorControl3Update = 0;
    }
}

/******************************************************************************
 * FunctionName : timeControl
 * Description  : com接收设置指令
 * Parameters   : void
 * Returns      : void
*******************************************************************************/
static uint8_t flag_actived = 0;
static uint32_t count_max = 0;
static uint32_t count = 0;
TimeTableNode *found;
static TimeInfo timeinfo_compare = {
   .year = 0,
	 .month = 0,
	 .date = 0,
	 .hour = 0,
	 .minute = 0,
};
void timeControl(void)
{
  
   found = findNode(timeTable, now_time.year, now_time.month, now_time.date, now_time.hour, now_time.minute);
   if (found 
		   && now_time.second == 0 &&
	  (timeinfo_compare.year!= found->year
	     || timeinfo_compare.month!= found->month
         || timeinfo_compare.date!= found->date
         || timeinfo_compare.hour!= found->hour
         || timeinfo_compare.minute!= found->minute) ) 
	{
       
		timeinfo_compare.year = found->year;
		timeinfo_compare.month = found->month;
		timeinfo_compare.date = found->date;
		timeinfo_compare.hour = found->hour;
		timeinfo_compare.minute = found->minute;
			
		printf("start_timer!!!\n");
		
		count_max = ((uint32_t)found->duration)*5;

		flag_actived = 1;	 
		// HAL_GPIO_WritePin(GPIOC, GPIO_PIN_13, GPIO_PIN_SET);


		motorControlStart(found);
		if (xSendUdpTaskSem != NULL) {
         xSemaphoreGive(xSendUdpTaskSem); // 初始状态为可触发 触发存储任务
    }
   }

	if(flag_actived == 1)
	{
		if(count < count_max && count_max > 0)
		{
			count ++;
		}
		else
		{
			count = 0;
			count_max = 0;
			flag_actived = 0;
			printf("stop_timer!!!\n");
			// HAL_GPIO_WritePin(GPIOC, GPIO_PIN_13, GPIO_PIN_RESET);

		    motorControlStart(NULL);
			if (xSendUdpTaskSem != NULL) {
					 xSemaphoreGive(xSendUdpTaskSem); // 初始状态为可触发 触发存储任务
					 }
			}
	}
}


void taskMotorDirectionControl(uint8_t motorNum, MotorDirection direction)
{
	switch(motorNum)
 {
 	case 1:
 		switch(direction)
 		{
 			case MotorPositive:
 				HAL_GPIO_WritePin(Motor1Direction_GPIO_Port, Motor1Direction_Pin, GPIO_PIN_SET);
 				break;
 			case MotorNegative:
 				HAL_GPIO_WritePin(Motor1Direction_GPIO_Port, Motor1Direction_Pin, GPIO_PIN_RESET);
				break;
			default:
			    break;
 		}
		break;
 	case 2:
 		switch(direction)
 		{
 			case MotorPositive:
 				HAL_GPIO_WritePin(Motor2Direction_GPIO_Port, Motor2Direction_Pin, GPIO_PIN_SET);
 				break;
 			case MotorNegative:
 				HAL_GPIO_WritePin(Motor2Direction_GPIO_Port, Motor2Direction_Pin, GPIO_PIN_RESET);
				break;
			default:
			    break;
 		}
		break;
 	case 3:
 		switch(direction)
 		{
 			case MotorPositive:
 				HAL_GPIO_WritePin(Motor3Direction_GPIO_Port, Motor3Direction_Pin, GPIO_PIN_SET);
 				break;
 			case MotorNegative:
 				HAL_GPIO_WritePin(Motor3Direction_GPIO_Port, Motor3Direction_Pin, GPIO_PIN_RESET);
				break;
			default:
			    break;
 		}
		break;
	default:
	    break;
 }
}


void taskMotorActionControl(uint8_t motorNum, MotorAction action)
{
	switch(motorNum)
 {
 	case 1:
 		switch(action)
 		{
 			case MotorAction_Start:
 				HAL_GPIO_WritePin(Motor1Bake_GPIO_Port, Motor1Bake_Pin, GPIO_PIN_SET);
 				break;
 			case MotorAction_Stop:
 				HAL_GPIO_WritePin(Motor1Bake_GPIO_Port, Motor1Bake_Pin, GPIO_PIN_RESET);
				break;
			default:
			    break;
 		}
		break;
 	case 2:
 		switch(action)
 		{
 			case MotorAction_Start:
 				HAL_GPIO_WritePin(Motor2Bake_GPIO_Port, Motor2Bake_Pin, GPIO_PIN_SET);
 				break;
 			case MotorAction_Stop:
 				HAL_GPIO_WritePin(Motor2Bake_GPIO_Port, Motor2Bake_Pin, GPIO_PIN_RESET);
				break;
			default:
			    break;
 		}
		break;
 	case 3:
 		switch(action)
 		{
 			case MotorAction_Start:
 				HAL_GPIO_WritePin(Motor3Bake_GPIO_Port, Motor3Bake_Pin, GPIO_PIN_SET);
 				break;
 			case MotorAction_Stop:
 				HAL_GPIO_WritePin(Motor3Bake_GPIO_Port, Motor3Bake_Pin, GPIO_PIN_RESET);
				break;
			default:
			    break;
 		}
		break;
	default:
	    break;
 }
}

void sendudp(void)
{
	static uint8_t motorControl1Action_buffer = 0;
	static uint8_t motorControl2Action_buffer = 0;
	static uint8_t motorControl3Action_buffer = 0;
	static uint8_t motorDirectionControl1Action_buffer = 0;
	static uint8_t motorDirectionControl2Action_buffer = 0;
	static uint8_t motorDirectionControl3Action_buffer = 0;
	static uint8_t motorSpeedControl1Action_buffer = 100;
	static uint8_t motorSpeedControl2Action_buffer = 100;
	static uint8_t motorSpeedControl3Action_buffer = 100;

	if(motorControl1Action != motorControl1Action_buffer)
	{
		motorControl1Action_buffer = motorControl1Action;
		if (xSendUdpTaskSem != NULL) {
         xSemaphoreGive(xSendUdpTaskSem); // 初始状态为可触发 触发存储任务
         }

	}

	if(motorControl2Action != motorControl2Action_buffer)
	{
		motorControl2Action_buffer = motorControl2Action;
		if (xSendUdpTaskSem != NULL) {
         xSemaphoreGive(xSendUdpTaskSem); // 初始状态为可触发 触发存储任务
         }

	}

	if(motorControl3Action != motorControl3Action_buffer)
	{
		motorControl3Action_buffer = motorControl3Action;
		if (xSendUdpTaskSem != NULL) {
         xSemaphoreGive(xSendUdpTaskSem); // 初始状态为可触发 触发存储任务
         }

	}

	if(motorDirectionControl1Action != motorDirectionControl1Action_buffer)
	{
		motorDirectionControl1Action_buffer = motorDirectionControl1Action;
		if (xSendUdpTaskSem != NULL) {
         xSemaphoreGive(xSendUdpTaskSem); // 初始状态为可触发 触发存储任务
         }
	}

	if(motorDirectionControl2Action != motorDirectionControl2Action_buffer)
	{
		motorDirectionControl2Action_buffer = motorDirectionControl2Action;
		if (xSendUdpTaskSem != NULL) {
         xSemaphoreGive(xSendUdpTaskSem); // 初始状态为可触发 触发存储任务
         }
	}

	if(motorDirectionControl3Action != motorDirectionControl3Action_buffer)
	{
		motorDirectionControl3Action_buffer = motorDirectionControl3Action;
		if (xSendUdpTaskSem != NULL) {
         xSemaphoreGive(xSendUdpTaskSem); // 初始状态为可触发 触发存储任务
         }
	}

	if(motorSpeedControl1Action != motorSpeedControl1Action_buffer)
	{
		motorSpeedControl1Action_buffer = motorSpeedControl1Action;
		if (xSendUdpTaskSem != NULL) {
         xSemaphoreGive(xSendUdpTaskSem); // 初始状态为可触发 触发存储任务
         }
	}

	if(motorSpeedControl2Action != motorSpeedControl2Action_buffer)
	{
		motorSpeedControl2Action_buffer = motorSpeedControl2Action;
		if (xSendUdpTaskSem != NULL) {
         xSemaphoreGive(xSendUdpTaskSem); // 初始状态为可触发 触发存储任务
         }
	}

	if(motorSpeedControl3Action != motorSpeedControl3Action_buffer)
	{
		motorSpeedControl3Action_buffer = motorSpeedControl3Action;
		if (xSendUdpTaskSem != NULL) {
         xSemaphoreGive(xSendUdpTaskSem); // 初始状态为可触发 触发存储任务
         }
	}
}


void motorControlStart(TimeTableNode *found)
{
    if(found != NULL)
		{
			motorSpeedControl1Action = found->speedA;
			motorSpeedControl2Action = found->speedB;
			motorSpeedControl3Action = 50; 

			motorControl1Action = 1;
			motorControl2Action = 1;
			motorControl3Action = 1;

			motorDirectionControl1Action = 1;
			motorDirectionControl2Action = 1;
			motorDirectionControl3Action = 1;

			TIM1_PWM_SetFrequencyAndDutyCycle(16000,motorSpeedControl1Action);	
		    TIM2_PWM_SetFrequencyAndDutyCycle(16000,motorSpeedControl2Action);	
		    TIM4_PWM_SetFrequencyAndDutyCycle(16000,motorSpeedControl3Action);	
			

			taskMotorActionControl(1,(MotorAction)motorControl1Action);
			taskMotorActionControl(2,(MotorAction)motorControl2Action);
			taskMotorActionControl(3,(MotorAction)motorControl3Action);

			taskMotorDirectionControl(1,(MotorDirection)motorDirectionControl1Action);
			taskMotorDirectionControl(2,(MotorDirection)motorDirectionControl2Action);
			taskMotorDirectionControl(3,(MotorDirection)motorDirectionControl3Action);
		}
		else
		{
			motorSpeedControl1Action = 100;
			motorSpeedControl2Action = 100;
			motorSpeedControl3Action = 100; 

			motorControl1Action = 0;
			motorControl2Action = 0;
			motorControl3Action = 0;

			motorDirectionControl1Action = 1;
			motorDirectionControl2Action = 1;
			motorDirectionControl3Action = 1;

			TIM1_PWM_SetFrequencyAndDutyCycle(16000,motorSpeedControl1Action);	
			TIM2_PWM_SetFrequencyAndDutyCycle(16000,motorSpeedControl2Action);	
			TIM4_PWM_SetFrequencyAndDutyCycle(16000,motorSpeedControl3Action);	
			

			taskMotorActionControl(1,(MotorAction)motorControl1Action);
			taskMotorActionControl(2,(MotorAction)motorControl2Action);
			taskMotorActionControl(3,(MotorAction)motorControl3Action);

			taskMotorDirectionControl(1,(MotorDirection)motorDirectionControl1Action);
			taskMotorDirectionControl(2,(MotorDirection)motorDirectionControl2Action);
			taskMotorDirectionControl(3,(MotorDirection)motorDirectionControl3Action);
		}
}
